﻿using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Drawing;
using System.Linq;
using System.IO.Ports;
using System.Threading;
using System.Windows.Forms;

using RoboticsConnection.Serializer;
using RoboticsConnection.Serializer.Components;
using RoboticsConnection.Serializer.Controllers;
using RoboticsConnection.Serializer.Ids;
using RoboticsConnection.Serializer.Sensors;
using Timer=System.Windows.Forms.Timer;

namespace StingerRobot
{
	public partial class Form1 : Form
	{
		private Serializer _serializer;
		private PIDMotorController _pmc;
		private Timer _timer;
		private bool _checkPid;

		private Collection<GP2D12> _irSensors;

		private const int _frequency = 50;
		bool _isInAutonomousMode = false;
		public Form1()
		{
			InitializeComponent();
		}

		private void Form1_Load(object sender, EventArgs e)
		{
			foreach (string port in SerialPort.GetPortNames())
			{
				ComListBox.Items.Add(port);
				ComListBox.Text = port;
			}

			gbSensorData.Enabled = false;
		}

		private void btnConnect_Click(object sender, EventArgs e)
		{
			if (null == _serializer)
			{
				InitializeSerializerObjects();
				btnConnect.Text = "Disconnect";
				gbSensorData.Enabled = true;
			}
			else
			{
				ShutdownSerializer();

				gbSensorData.Enabled = false;
				btnConnect.Text = "Connect";
			}
		}

		private void InitializeSerializerObjects()
		{
			_serializer = new Serializer
							 {
								 BaudRate = 19200,
								 PortName = ComListBox.Text
							 };

			var batteryVoltage = new AnalogSensor(_serializer)
								 {
									 Pin = AnalogPinId.Pin5,
									 UpdateFrequency = 1000
								 };
			batteryVoltage.ValueChanged += batteryVoltage_ValueChanged;

			_pmc = new PIDMotorController(_serializer)
					  {
						  // Set velocity and distance PID parameters:
						  VelProportional = 10,
						  VelIntegral = 0,
						  VelDerivative = 5,
						  VelLoop = 20,
						  DistProportional = 1,
						  DistIntegral = 0,
						  DistDerivative = 0,
						  DistAcceleration = 1,
						  DistDeadband = 5,
						  DeadbandEnabled = true,
						  // Set Physical drivetrain configuration parameters (for Stinger).
						  EncoderResolution = 12,
						  GearReduction = 0.0185,  // 1/54
						  TicksPerRevolution = 624, // Gear Reduction * Encoder Resolution = 52 * 12
						  WheelDiameter = 2.375,
						  WheelTrack = 8.6, // Distance between centerline of drive tracks
						  VelocityDivider = 2.857
					  };


			_irSensors = new Collection<GP2D12>
			             	{
			             		CreateIRSensor(AnalogPinId.Pin0, _frequency, .5),
			             		CreateIRSensor(AnalogPinId.Pin1, _frequency, .5),
			             		CreateIRSensor(AnalogPinId.Pin2, _frequency, .5)
			             	};
			CreateAirTempSensor(AnalogPinId.Pin3, _frequency, 1);
			CreateCompass();

			_serializer.StartCommunication();
			_timer = new Timer { Interval = 100 };
			_timer.Tick += timer_Tick;
			_timer.Start();
		}

		private void ShutdownSerializer()
		{
			if (null != _timer)
				_timer.Dispose();

			if (null != _serializer)
				_serializer.ShutDown();

			_serializer = null;
		}

		#region compass
		private CMPS03 CreateCompass()
		{
			var compass = new CMPS03(_serializer)
							  {
								  UpdateFrequency = _frequency,
								  Enabled = true
							  };
			compass.HeadingChanged += compass_HeadingChanged;
			return compass;
		}

		private void compass_HeadingChanged(SerializerComponent sender)
		{
			if (_isInAutonomousMode || null == sender || sender.GetType() != typeof(CMPS03))
				return;

			angleSelect1.Angle = ((CMPS03)sender).Heading;// +135;
		}
		#endregion
		#region tempature
		private AmbientTemperatureSensor CreateAirTempSensor(AnalogPinId id, int updateFrequency, int threshold)
		{
			var temp = new AmbientTemperatureSensor(_serializer)
						   {
							   Pin = id,
							   UpdateFrequency = updateFrequency,
							   TemperatureChangedThreshold = threshold
						   };
			temp.TemperatureChanged += temp_TemperatureChanged;

			return temp;
		}

		private void temp_TemperatureChanged(SerializerComponent sender)
		{
			if (_isInAutonomousMode || null == sender || sender.GetType() != typeof(AmbientTemperatureSensor))
				return;

			var temp = ((AmbientTemperatureSensor)sender);
			var val = String.Format("{0:0.0} F", temp.Temperature);

			txtTemp.Text = val;
		}
		#endregion
		#region IR
		private GP2D12 CreateIRSensor(AnalogPinId id, int updateFrequency, double threshold)
		{
			var ir = new GP2D12(_serializer)
						 {
							 Pin = id,
							 UpdateFrequency = updateFrequency,
							 DistanceChangedThreshold = threshold
						 };

			ir.DistanceChanged += Gp2d12_DistanceChanged;

			return ir;
		}

		private void Gp2d12_DistanceChanged(SerializerComponent sender)
		{
			if (_isInAutonomousMode || null == sender || sender.GetType() != typeof(GP2D12))
				return;

			var ir = ((GP2D12)sender);
			var val = String.Format("{0:0.00} inches", ir.Distance);

			TextBox txt0 = null;
			TextBox txt1 = null;
			switch (ir.Pin)
			{
				case AnalogPinId.Pin0:
					txt0 = txtSensorLeft0;
					txt1 = txtSensorLeft1;
					break;
				case AnalogPinId.Pin1:
					txt0 = txtSensorCenter0;
					txt1 = txtSensorCenter1;
					break;
				case AnalogPinId.Pin2:
					txt0 = txtSensorRight0;
					txt1 = txtSensorRight1;
					break;
			}

			if (null == txt0 || null == txt1)
				return;

			txt0.Text = val;
			txt1.Text = val;
		}
		#endregion
		private void timer_Tick(object sender, EventArgs e)
		{
			_serializer.PumpEvents();

			if (!_checkPid)
				return;

			if (_pmc.QueryStatus())
			{
				txtStatus.ForeColor = Color.Red;
				txtStatus.Text = "Busy";
			}
			else
			{
				txtStatus.ForeColor = Color.Green;
				txtStatus.Text = "Complete";
				_checkPid = false;
			}
		}

		private void batteryVoltage_ValueChanged(SerializerComponent sender)
		{
			if (_isInAutonomousMode || null == sender || sender.GetType() != typeof(AnalogSensor))
				return;

			var batteryVoltage = ((AnalogSensor)sender);
			// Calculate actual battery voltage: 
			var voltage = batteryVoltage.Value * 0.01459;
			txtBattery.Text = String.Format("{0:0.0} volts", voltage);
		}

		private void btnMove_Click(object sender, EventArgs e)
		{
			_pmc.Speed = tbarSpeed.Value;
			_pmc.Distance = (double)numMotorDistance.Value;
			_pmc.TravelDistance();
			_checkPid = true;
		}

		private void btnRotate_Click(object sender, EventArgs e)
		{
			_pmc.RotationAngle = tbarRotate.Value;
			if (_pmc.RotationAngle != 0)
				return;

			_pmc.Speed = tbarSpeed.Value;
			_pmc.Rotate();
			_checkPid = true;
		}

		private void Form1_FormClosing(object sender, FormClosingEventArgs e)
		{
			ShutdownSerializer();
		}

		private void tbarRotate_Scroll(object sender, EventArgs e)
		{
			lblRotate.Text = "Rotate: " + tbarRotate.Value;
		}

		
		private void btnAutonomous_Click(object sender, EventArgs e)
		{
			if (!_isInAutonomousMode)
			{
				_timer.Enabled = false;

				_pmc.Speed = tbarSpeed.Value;
				ThreadPool.QueueUserWorkItem(AutonomousMode);
				_isInAutonomousMode = true;

				btnAutonomous.Text = "Stop Moving and Avoiding";
			}
			else
			{
				_timer.Enabled = true;

				_isInAutonomousMode = false;
				btnAutonomous.Text = "Move and Avoid";
			}
		}


		private const int furthestDistance = 8;
		private void AutonomousMode(object state)
		{
			while (_isInAutonomousMode)
			{
				_serializer.PumpEvents();

				int left = (int)_irSensors[0].Distance;
				int center = (int)_irSensors[1].Distance;
				int right = (int)_irSensors[2].Distance;

				bool obstacleLeft = left < furthestDistance;
				bool obstacleCenter = center < furthestDistance;
				bool obstacleRight = right < furthestDistance;
				bool obstaclePresent = obstacleLeft || obstacleCenter || obstacleRight;

				if (obstaclePresent)
				{
					if (obstacleLeft)
						_pmc.RotationAngle = 30;
					else if (obstacleRight)
						_pmc.RotationAngle = -30;
					else
					{
						_pmc.RotationAngle = 45 * ((left < right) ? 1 : -1);
					}
					_pmc.Rotate();
				}

				while (_pmc.QueryStatus())
				{
					Thread.Sleep(100);
				}

				_pmc.TravelAtSpeed();
				Thread.Sleep(100);
			}
			_pmc.Stop();
		}
	}
}